﻿using System;
using Gameplay.PVE;
using System.Collections.Generic;
using LogSystem;
using NpcFramework;
using UnityEngine;
using UnityEngine.Serialization;

namespace Yoozoo.Gameplay.Liberty.AI
{
    public enum SpeedStrategyType
    {
        Const = 1,
        Curve = 2,
        RobCar = 3,
    }

    [Serializable]
    public class SpeedStrategyContainer
    {
        public SpeedStrategyType speedType;
        public SpeedStrategy speedStrategy;
    }
    
    public abstract class AISeeker : MonoBehaviour
    {
        public Transform Transform;
        public CustomPath aiPath;
        [SerializeField]
        protected float savedMaxSpeed;
        public bool IsMotionSeeker = false;
        [HideInInspector]
        public LocomotionAgent LocomotionAgent;
        /// <summary>
        /// Transform of our game object
        /// </summary>
        public Transform _Transform = null;
        [SerializeField]
        protected Vector3 _destination;


        public Transform CameraTransform;
        /// <summary>
        /// Provides an error amount for determining distance
        /// </summary>
        public const float EPSILON = 0.01f;
        /// <summary>
        /// Distance we'll use to start slowing down so we can arrive nicely.
        /// </summary>
        public float _SlowDistance = 4.0f;
        public float SlowDistance
        {
            get { return _SlowDistance; }
            set { _SlowDistance = value; }
        }

        public SpeedStrategy speedStrategy;
        public List<SpeedStrategyContainer> speedStrategies;
        private Dictionary<SpeedStrategyType, SpeedStrategy> _speedStrategyMap;
        
        public bool _IsEnabled = true;

        public bool isWait = false;


        /// <summary>
        /// Desired degrees of rotation per second
        /// </summary>
        public float _MaxViewSpeed = 1f;
        public float MaxViewSpeed
        {
            get { return _MaxViewSpeed; }

            set
            {
                _MaxViewSpeed = value;
                mViewSpeedPer60FPSTick = _MaxViewSpeed * 60f;
            }
        }
        /// <summary>
        /// Speed we'll actually apply to the view component. This is essentially the
        /// value tick assuming we're running at 60 FPS
        /// </summary>
        protected float mViewSpeedPer60FPSTick = 1f;
        /// <summary>
        /// Speed we'll ultimately reduce to before stopping
        /// </summary>
        /// 
        public bool IsViewingActivated
        {
            get
            {
                return mViewX != 0f;
            }
        }
        // public float _SlowFactor = 0.5f;
        // public float SlowFactor
        // {
        //     get { return _SlowFactor; }
        //     set { _SlowFactor = value; }
        // }
        /// <summary>
        /// Retrieves horizontal view movement from the the input
        /// </summary>
        protected float mViewX = 0f;
        public float ViewX
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mViewX;
            }
        }

        /// <summary>
        /// Retrieves vertical view movement from the the input
        /// </summary>
        protected float mViewY = 0f;
        public float ViewY
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mViewY;
            }
        }
        /// <summary>
        /// Retrieves horizontal movement from the the input
        /// </summary>
        [SerializeField]
        protected float mMovementX = 0f;
        public float MovementX
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mMovementX;
            }
        }
        /// <summary>
        /// Height of the path from the actual navmesh surface. This is
        /// This height is added to the path by unity
        /// </summary>
        public float _PathHeight = 0.05f;
        /// <summary>
        /// Retrieves vertical movement from the the input
        /// </summary>
        [SerializeField]
        protected float mMovementY = 0f;
        private float TargetNormalizedSpeed;

        public float MovementY
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mMovementY;
            }
        }
        /// <summary>
        /// Set by an external object, it tracks the angle of the
        /// user input compared to the avatars's forward direction
        /// Note that this info isn't reliable as objects using it 
        /// before it's set it will get float.NaN.
        /// </summary>
        protected float mInputFromAvatarAngle = 0f;
        public float InputFromAvatarAngle
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mInputFromAvatarAngle;
            }

            set { }
        }
        /// <summary>
        /// Set by an external object, it tracks the angle of the
        /// user input compared to the camera's forward direction
        /// Note that this info isn't reliable as objects using it 
        /// before it's set it will get float.NaN.
        /// </summary>
        protected float mInputFromCameraAngle = 0f;
        public float InputFromCameraAngle
        {
            get
            {
                if (!_IsEnabled) { return 0f; }
                return mInputFromCameraAngle;
            }

            set { }
        }
        /// <summary>
        /// Determines if we've arrived at the final destination
        /// </summary>
        protected bool mHasArrived = false;
        public bool HasArrived
        {
            get { return mHasArrived; }
        }
        private bool mFirstPathSet;
        protected bool mFirstPathValid = false;
        protected Vector3 mAgentDestination = new Vector3(float.MinValue, float.MinValue, float.MinValue);
        /// <summary>
        /// Determines if the current path is valid
        /// </summary>
        protected bool mIsPathValid = true;
        /// <summary>
        /// Direction we're traveling to
        /// </summary>
        protected Vector3 mTargetVector = Vector3.zero;

        /// <summary>
        /// Distance between the current position and actual target
        /// </summary>
        protected float mTargetDistance = 0f;
        public Vector3 _TargetPosition = Vector3.zero;

        /// <summary>
        /// Set when we're within the slow distance
        /// </summary>
        protected bool mIsInSlowDistance = false;
        public bool IsInSlowDistance
        {
            get { return mIsInSlowDistance; }
        }
        /// <summary>
        /// Determines if a target is currently set
        /// </summary>
        protected bool mIsTargetSet = false;
        public bool IsTargetSet
        {
            get { return mIsTargetSet; }
        }
        protected Vector3 mWaypoint = Vector3.zero;
        /// <summary>
        /// Diection we're traveling to
        /// </summary>
        protected Vector3 mWaypointVector = Vector3.zero;
        public Vector3 WaypointVector => mWaypointVector;
        /// <summary>
        /// Distance between the current position and temporary target
        /// </summary>
        protected float mWaypointDistance = 0f;
        public float WaypointDistance => mWaypointDistance;
        /// <summary>
        /// 是否到达路径尽头（终点）
        /// </summary>
        /// <returns></returns>
        public abstract bool ReachedEndOfPath();

        public virtual void OnPlayerRadarUpdate(AIEntity aiEntity)
        {
        }

        public virtual void OnEnterPlayerRadar(AIEntity aiEntity)
        {
        }
        
        public virtual void OnExitPlayerRadar(AIEntity aiEntity)
        {
        }

        // public virtual void ResetDestination()
        // {
        //     _destination = NFMgr.DataManager.GetNpcData(ID).Destination;
        // }
        protected void OnAwake()
        {
            // Default the speed we'll use to rotate
            mViewSpeedPer60FPSTick = _MaxViewSpeed * 60f;
            savedMaxSpeed = aiPath.maxSpeed;
            
            _speedStrategyMap = new Dictionary<SpeedStrategyType, SpeedStrategy>();
            if (speedStrategies!=null)
            {
                foreach (var strategyContainer in speedStrategies)
                {
                    if (strategyContainer!=null)
                    {
                        _speedStrategyMap[strategyContainer.speedType] = strategyContainer.speedStrategy;
                    }
                }
            }

        }

        public int seekFrame = 0;

        public void OnLateUpdate(bool needSimulateInput, float deltaTime)
        {
            // Reset our input values
            mViewX = 0f;
            mViewY = 0f;
            mMovementX = 0f;
            mMovementY = 0f;
            mInputFromAvatarAngle = 0f;
            mInputFromCameraAngle = 0f;

            // Check if our first path is set and done
            if (mFirstPathSet && aiPath.hasPath && !aiPath.pathPending)
            {
                mFirstPathValid = true;
            }

            //SetDestination(_TargetPosition);

            // Determine if we're at the destination
            mTargetVector = mAgentDestination - _Transform.position;
            mTargetDistance = mTargetVector.magnitude;

            // Check if we've arrived
            if (mTargetDistance < aiPath.endReachedDistance)
            {
                ClearTarget();
                mHasArrived = true;
                mFirstPathSet = false;
                mFirstPathValid = false;

                OnArrived();
                seekFrame = 0;
                //aiPath.NextPosition = _Transform.position;
                aiPath.FinalizeMovement(_Transform.position, _Transform.rotation);
            }

            // Determine the next move
            if (!mHasArrived && mFirstPathValid)
            {
                // If we've reset the path, we may not be ready for a new steering target yet
                if (aiPath.hasPath && !aiPath.pathPending)
                {
                    seekFrame++;
                    mIsPathValid = true;

                    mWaypoint = aiPath.steeringTarget;

                    // TRT 09/25/2017 - Replaced for obstacle avoidance
                    //mWaypointVector = mWaypoint - _Transform.position;
                    mWaypointVector = aiPath.desiredVelocity;

                    mWaypointDistance = mWaypointVector.magnitude;
                    //Debug.LogError("id:" + ID + "     " + mWaypointVector);

                    if (mTargetDistance > _SlowDistance) { mIsInSlowDistance = false; }
                }

                // Determine if we're within the slow distance. We only want to fire the event once
                if (_SlowDistance > 0f && mTargetDistance < _SlowDistance)
                {
                    if (!mIsInSlowDistance) { OnSlowDistanceEntered(); }
                    mIsInSlowDistance = true;
                }

                // Using the waypoint information, we want to simulate the input 

                if (needSimulateInput)
                {
                    SimulateInput(deltaTime);  // 不能只用路径点，还应该用模拟行走的位置点来限制大小，这个只能决定方向，而且路径点密集的时候需要进行处理。
                }
            }



        }

        public void RegisterAgent(Transform transform, LocomotionAgent agent)
        {
            _Transform = transform;
            //CameraTransform = LibertyAIUtils.PlayerCamera.transform;
            IsMotionSeeker = true;
            LocomotionAgent = agent;
        }
        
        public void SetForceUpdateTargetRotation(Quaternion rotation)
        {
            if (LocomotionAgent)
            {
                LocomotionAgent.SetForceUpdateTargetRotation(rotation);
            }
        }
        public void SetForceUpdateTargetRotationAndPosition(Quaternion rotation,Vector3 position)
        {
            if (LocomotionAgent)
            {
                LocomotionAgent.SetForceUpdateTargetRotationAndPosition(rotation,position);
            }
        }
        /// <summary>
        /// Converts the nav mesh agent data into psuedo-input that the motion controller
        /// will use to drive animations.
        /// </summary>
        protected void SimulateInput(float deltaTime)
        {
            float lDeltaTime = deltaTime;// TimeManager.SmoothedDeltaTime;

            // Direction we need to travel in
            Vector3 lDirection = mWaypointVector;
            lDirection.y = 0;
            lDirection.Normalize();

            // Determine our view
            Vector3 lVerticalDirection = Vector3.Project(lDirection, _Transform.up);
            Vector3 lLateralDirection = lDirection - lVerticalDirection;
            mInputFromAvatarAngle = SignedAngle(_Transform.forward, lLateralDirection);

            // Determine how much we simulate the view x. We temper it to make it smooth
            float lYawAngleAbs = Mathf.Min(Mathf.Abs(mInputFromAvatarAngle), mViewSpeedPer60FPSTick * lDeltaTime);
            if (lYawAngleAbs < EPSILON) { lYawAngleAbs = 0f; }

            //Debug.LogError("mInputFromAvatarAngle:" + mInputFromAvatarAngle);
            //Debug.LogError("mWaypointVector:" + mWaypointVector);
            mViewX = Mathf.Sign(mInputFromAvatarAngle) * lYawAngleAbs;

            // Determine our movement
            float originSpeed = savedMaxSpeed;
            if (speedStrategy == null)
            {
                mMovementY = originSpeed;
            }
            else
            {
                mMovementY = speedStrategy.GetSpeed(originSpeed, mTargetVector);
            }

            aiPath.maxSpeed = mMovementY;
            
            // if (mTargetDistance > 0.1f)//aiPath.endReachedDistance)
            // {
            //     // Calculate our own slowing
            //     float lRelativeMoveSpeed = aiPath.maxSpeed;
            //     // if (mIsInSlowDistance && _SlowFactor > 0f)
            //     // {
            //     //     float lSlowPercent = (mTargetDistance - aiPath.endReachedDistance) / (_SlowDistance - aiPath.endReachedDistance);
            //     //     lRelativeMoveSpeed = ((1f - _SlowFactor) * lSlowPercent) + _SlowFactor;
            //     // }
            //
            //     // TRT 4/5/2016: Force the slow distance as an absolute value
            //     if (mIsInSlowDistance && _SlowFactor > 0f)
            //     {
            //         lRelativeMoveSpeed = _SlowFactor;
            //     }
            //
            //     mMovementY = 1f * lRelativeMoveSpeed;
            //
            //     TargetNormalizedSpeed = lRelativeMoveSpeed;
            // }

            // Grab extra input information if we can
            if (CameraTransform == null)
            {
                mInputFromCameraAngle = mInputFromAvatarAngle;
            }
            else
            {
                Vector3 lInputForward = new Vector3(mMovementX, 0f, mMovementY);

                // We do the inverse tilt so we calculate the rotation in "natural up" space vs. "actor up" space. 
                Quaternion lInvTilt = FromToRotation(_Transform.up, Vector3.up);

                // Camera forward in "natural up"
                Vector3 lCameraForward = lInvTilt * CameraTransform.forward;

                // Create a quaternion that gets us from our world-forward to our camera direction.
                Quaternion lToCamera = Quaternion.LookRotation(lCameraForward, lInvTilt * _Transform.up);

                // Transform joystick from world space to camera space. Now the input is relative
                // to how the camera is facing.
                Vector3 lMoveDirection = lToCamera * lInputForward;
                mInputFromCameraAngle = GetHorizontalAngle(lCameraForward, lMoveDirection);
            }


            ProcessSimulatedInput();
        }

        private void ProcessSimulatedInput()
        {

        }

        /// <summary>
        /// Event function for when we arrive at the destination
        /// </summary>
        protected virtual void OnArrived()
        {
        }
        public virtual void ResetTargetEndStatus(bool status)
        {

        }

        public virtual void SetPosition(Vector3 pos)
        {
            Transform.position = pos;
        }
        
        public virtual void SwitchSpeedStrategy(SpeedStrategyType strategyType)
        {
            _speedStrategyMap.TryGetValue(strategyType, out SpeedStrategy strategy);
            if (strategy)
            {
                speedStrategy = strategy;
            }
            else
            {
                Debug.LogError(LogModule.LibertyScene,$"没有这个速度策略类型：{strategyType}");
            }
        }
        
        public virtual void SetDestination(Vector3 destination)
        {
            seekFrame = 0;
            // Reset the properties
            mHasArrived = false;
            
            // Set the new destination
            mAgentDestination = destination;
            _TargetPosition = destination;
            if (IsMotionSeeker)
            {
                mIsPathValid = false;
                aiPath.isStopped = false;
                _destination = destination;
                aiPath.destination = destination;
                aiPath.SearchPath();
                mFirstPathSet = true;
            }
            else
            {
                _destination = destination;
                aiPath.destination = destination;
                aiPath.SearchPath();
            }
        }

        /// <summary>
        /// 停止寻路
        /// </summary>
        public virtual void Stop()
        {
            mHasArrived = true;
            if (IsMotionSeeker)
            {
                aiPath.isStopped = true;
            }

            aiPath.SetPath(null);
        }
        
        public virtual void SetMaxSpeed(float speed)
        {

        }

        public virtual void SetPriority(float priority)
        {
            
        }
        
        /// <summary>
        /// Event function for when we are within the slow distance
        /// </summary>
        protected virtual void OnSlowDistanceEntered()
        {
        }
        
        /// <summary>
        /// Clears all the target properties
        /// </summary>
        public void ClearTarget()
        {
            _TargetPosition = new Vector3(float.MinValue, float.MinValue, float.MinValue);
            aiPath.isStopped = true;
            mHasArrived = false;
            mIsPathValid = true;
            mFirstPathSet = false;
            mFirstPathValid = false;
            mIsInSlowDistance = false;
            mIsTargetSet = false;

            mViewX = 0f;
            mViewY = 0f;
            mMovementX = 0f;
            mMovementY = 0f;
            mInputFromAvatarAngle = 0f;
            mInputFromCameraAngle = 0f;
        }
        /// <summary>
        /// Determines the signed angle required to go from one vector to another.
        /// </summary>
        /// <param name="rThis">The source vector</param>
        /// <param name="rTarget">The destination vector</param>
        /// <returns></returns>
        private float SignedAngle(Vector3 rFrom, Vector3 rTo)
        {
            if (rTo == rFrom) { return 0f; }

            Vector3 lCross = Vector3.Cross(rFrom, rTo);
            float lSign = (lCross.y < -0.0001f ? -1 : 1);

            float lDot = Vector3.Dot(rFrom, rTo);

            return lSign * Mathf.Atan2(lCross.magnitude, lDot) * Mathf.Rad2Deg;
        }

        /// <summary>
        /// Handles smaller values better than Unity's version.
        /// </summary>
        /// <param name="u"></param>
        /// <param name="v"></param>
        /// <returns></returns>
        private Quaternion FromToRotation(Vector3 u, Vector3 v)
        {
            float lTheta = Vector3.Dot(u.normalized, v.normalized);

            if (lTheta >= 1f)
            {
                return Quaternion.identity;
            }
            else if (lTheta <= -1f)
            {
                Vector3 lSimpleAxis = Vector3.Cross(u, Vector3.right);
                if (lSimpleAxis.sqrMagnitude == 0f) { lSimpleAxis = Vector3.Cross(u, Vector3.up); }

                return Quaternion.AngleAxis(180f, lSimpleAxis);
            }

            float lRadians = Mathf.Acos(lTheta);
            Vector3 lAxis = (Vector3.Cross(u, v)).normalized;

            return Quaternion.AngleAxis(lRadians * Mathf.Rad2Deg, lAxis);
        }

        /// <summary>
        /// Gets the horizontal angle between two vectors. The calculation
        /// removes any y components before calculating the angle.
        /// </summary>
        /// <returns>The signed horizontal angle (in degrees).</returns>
        /// <param name="rFrom">Angle representing the starting vector</param>
        /// <param name="rTo">Angle representing the resulting vector</param>
        private float GetHorizontalAngle(Vector3 rFrom, Vector3 rTo)
        {
            float lAngle = Mathf.Atan2(Vector3.Dot(Vector3.up, Vector3.Cross(rFrom, rTo)), Vector3.Dot(rFrom, rTo));
            lAngle *= Mathf.Rad2Deg;

            if (Mathf.Abs(lAngle) < 0.0001f) { lAngle = 0f; }

            return lAngle;
        }
        void OnDrawGizmos()
        {
            if (aiPath == null)
            {
                return;
            }
            Gizmos.color = Color.green;
            List<Vector3> list = new List<Vector3>();
            aiPath.GetRemainingPath(list, out var stale);
            for (int i = 1; i < list.Count; i++)
            {
                Gizmos.DrawLine(list[i - 1], list[i]);
            }
            Gizmos.color = Color.yellow;
            Gizmos.DrawSphere(aiPath.steeringTarget, 0.1f);

            Gizmos.color = Color.blue;
            Gizmos.DrawSphere(mWaypoint, 0.1f);
            Gizmos.DrawRay(transform.position, mTargetVector.normalized);

            Gizmos.color = Color.yellow;
            Gizmos.DrawWireSphere(mAgentDestination, _SlowDistance);

            Gizmos.color = Color.red;
            Gizmos.DrawRay(transform.position, mWaypointVector);


            Gizmos.color = Color.cyan;
            // Direction we need to travel in
            Vector3 lDirection = mWaypointVector;
            lDirection.y = 0;
            lDirection.Normalize();

            // Determine our view
            Vector3 lVerticalDirection = Vector3.Project(lDirection, transform.up);
            Vector3 lLateralDirection = lDirection - lVerticalDirection;
            Gizmos.DrawRay(transform.position, lLateralDirection);
        }
    }
}